/**
 * @file           : receive_process.cpp
 * @author         : fwy
 * @brief          :
 * @email          : 2643660853@qq.com
 * @date           : 23-1-18 上午1:06
 */

#include <ros/ros.h>
#include "serial_data.h"
#include "msg_process/receive_data.h"

using namespace std;
msg_process::receive_data temp_data;

int main(int argc, char **argv)
{
  // 初始化节点
  ros::init(argc, argv, "receive_process");
  // 句柄
  ros::NodeHandle nh;
  // 将串口收到的数据发送到serial_receive_data话题上
  ros::Publisher serial_receive_data_pub = nh.advertise<msg_process::receive_data>("/serial_receive_data", 1);
  ros::Rate loop_rate(100);
  while (nh.ok())
  {
    // 读取串口数据
    TransferData serial_data = serialdata.read();
    // 类型转换 TransferData转换为 serial_process::receive_data以发布
    temp_data.mode_L_hip = serial_data.data_u8[0];
    temp_data.mode_L_hip2 = serial_data.data_u8[1];
    temp_data.mode_L_thign = serial_data.data_u8[2];
    temp_data.mode_L_calf = serial_data.data_u8[3];
    temp_data.mode_L_toe = serial_data.data_u8[4];
    temp_data.mode_R_hip = serial_data.data_u8[5];
    temp_data.mode_R_hip2 = serial_data.data_u8[6];
    temp_data.mode_R_thign = serial_data.data_u8[7];
    temp_data.mode_R_calf = serial_data.data_u8[8];
    temp_data.mode_R_toe = serial_data.data_u8[9];

    temp_data.position_x = serial_data.data_f[0];
    temp_data.position_y = serial_data.data_f[1];
    temp_data.position_z = serial_data.data_f[2];
    temp_data.quaternion_w = serial_data.data_f[3];
    temp_data.quaternion_x = serial_data.data_f[4];
    temp_data.quaternion_y = serial_data.data_f[5];
    temp_data.quaternion_z = serial_data.data_f[6];
    temp_data.gyroscope_x = serial_data.data_f[7];
    temp_data.gyroscope_y = serial_data.data_f[8];
    temp_data.gyroscope_z = serial_data.data_f[9];
    temp_data.accelerometer_x = serial_data.data_f[10];
    temp_data.accelerometer_y = serial_data.data_f[11];
    temp_data.accelerometer_z = serial_data.data_f[12];

    temp_data.q_L_hip = serial_data.data_f[13];
    temp_data.dq_L_hip = serial_data.data_f[14];
    temp_data.tauEst_L_hip = serial_data.data_f[15];
    temp_data.q_L_hip2 = serial_data.data_f[16];
    temp_data.dq_L_hip2 = serial_data.data_f[17];
    temp_data.tauEst_L_hip2 = serial_data.data_f[18];
    temp_data.q_L_thign = serial_data.data_f[19];
    temp_data.dq_L_thign = serial_data.data_f[20];
    temp_data.tauEst_L_thign = serial_data.data_f[21];
    temp_data.q_L_calf = serial_data.data_f[22];
    temp_data.dq_L_calf = serial_data.data_f[23];
    temp_data.tauEst_L_calf = serial_data.data_f[24];
    temp_data.q_L_toe = serial_data.data_f[25];
    temp_data.dq_L_toe = serial_data.data_f[26];
    temp_data.tauEst_L_toe = serial_data.data_f[27];

    temp_data.q_R_hip = serial_data.data_f[28];
    temp_data.dq_R_hip = serial_data.data_f[29];
    temp_data.tauEst_R_hip = serial_data.data_f[30];
    temp_data.q_R_hip2 = serial_data.data_f[31];
    temp_data.dq_R_hip2 = serial_data.data_f[32];
    temp_data.tauEst_R_hip2 = serial_data.data_f[33];
    temp_data.q_R_thign = serial_data.data_f[34];
    temp_data.dq_R_thign = serial_data.data_f[35];
    temp_data.tauEst_R_thign = serial_data.data_f[36];
    temp_data.q_R_calf = serial_data.data_f[37];
    temp_data.dq_R_calf = serial_data.data_f[38];
    temp_data.tauEst_R_calf = serial_data.data_f[39];
    temp_data.q_R_toe = serial_data.data_f[40];
    temp_data.dq_R_toe = serial_data.data_f[41];
    temp_data.tauEst_R_toe = serial_data.data_f[42];

    ROS_WARN("++++++++++++++++++++++++++++++++++++++");
    ROS_WARN("quaternion_w : %d", temp_data.quaternion_w);
    ROS_WARN("quaternion_x : %d", temp_data.quaternion_x);
    ROS_WARN("quaternion_y : %d", temp_data.quaternion_y);
    ROS_WARN("quaternion_z : %d", temp_data.quaternion_z);
    ROS_WARN("gyroscope_x : %d", temp_data.gyroscope_x);
    ROS_WARN("gyroscope_y : %d", temp_data.gyroscope_y);
    ROS_WARN("gyroscope_z : %f", temp_data.gyroscope_z);
    ROS_WARN("accelerometer_x : %f", temp_data.accelerometer_x);
    ROS_WARN("accelerometer_y : %f", temp_data.accelerometer_y);
    ROS_WARN("accelerometer_z : %f", temp_data.accelerometer_z);
    ROS_WARN("++++++++++++++++++++++++++++++++++++++");
    serial_receive_data_pub.publish(temp_data);
    loop_rate.sleep();
  }
}
